| Note: This tutorial assumes that you have completed the previous tutorials: decision_making/Tutorials/CogniTAO(C++). |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Getting Started with TAO in C++
Description: TAO, Think As OneKeywords: decision_making,TAO
Tutorial Level: BEGINNER
Introduction
As described in CogniTAO BDI, a number of components (Plans, Tasks, Allocations) are written and designed by the programmer, while complying with the TAO Plans and hierarchy model. Here we will demonstrate how to put the pieces together, and create your first TAO program using those components, in an easy-to-catch tutorial that will cover most of what you need to understand, in order to comprehend the easy usage of TAO.
Running the example
roslaunch decision_making_examples tao_increment.launch
Components, Conditions And Code
This is taoIncrement.cpp - In the following code we demonstrate the TAO Incremental example (hold on - it looks like much - so we'll break it down to pieces in a moment...).
1 #include <iostream>
2 #include <boost/bind.hpp>
3 #include <boost/thread.hpp>
4 #include <boost/foreach.hpp>
5 #include <ros/ros.h>
6
7 #include <decision_making/TAO.h>
8 #include <decision_making/TAOStdProtocols.h>
9 #include <decision_making/ROSTask.h>
10 #include <decision_making/DecisionMaking.h>
11
12 using namespace std;
13 using namespace decision_making;
14
15 #define foreach BOOST_FOREACH
16
17 TAO_HEADER(Even)
18 TAO_HEADER(Odd)
19
20 //#define TAO_STOP_CONDITION(X) TAO_STOP_CONDITION_AND_PRINT_EVENTS(X)
21
22 struct WorldModel: public CallContextParameters{
23 int i;
24 string str()const{ stringstream s; s<<"i="<<i; return s.str(); }
25 };
26 #define WM call_ctx.parameters<WorldModel>()
27
28 TAO(Incrementer)
29 {
30 TAO_PLANS{
31 Increment,
32 }
33 TAO_START_PLAN(Increment);
34 TAO_BGN{
35 TAO_PLAN( Increment ){
36 TAO_START_CONDITION( WM.i < 100 );
37 TAO_ALLOCATE(AllocFirstReady){
38 TAO_SUBPLAN(Even);
39 TAO_SUBPLAN(Odd);
40 }
41 TAO_STOP_CONDITION( false );
42 TAO_NEXT(NextFirstReady){
43 TAO_NEXT_PLAN(Increment);
44 }
45 }
46 }
47 TAO_END
48 }
49
50 TAO(Even)
51 {
52 TAO_PLANS{
53 Even,
54 }
55 TAO_START_PLAN(Even);
56 TAO_BGN
57 {
58 TAO_PLAN( Even ){
59 TAO_START_CONDITION( WM.i%2 == 0 );
60 WM.i++;
61 cout<<"Even : "<<WM.str()<<endl;
62 sleep(1);
63 TAO_ALLOCATE_EMPTY
64 TAO_STOP_CONDITION(true);
65 TAO_NEXT_EMPTY
66 }
67 }
68 TAO_END
69 }
70
71 TAO(Odd)
72 {
73 TAO_PLANS{
74 Odd,
75 }
76 TAO_START_PLAN(Odd);
77 TAO_BGN
78 {
79 TAO_PLAN( Odd ){
80 TAO_START_CONDITION( WM.i%2 != 0 );
81 WM.i++;
82 cout<<"Odd : "<<WM.str()<<endl;
83 sleep(1);
84 TAO_ALLOCATE_EMPTY
85 TAO_STOP_CONDITION(true);
86 TAO_NEXT_EMPTY
87 }
88 }
89 TAO_END
90 }
91
92 decision_making::TaskResult testTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
93 cout<<"[testTask from "<<context.str()<<"]"<<endl;
94 sleep(1);
95 return TaskResult::SUCCESS();
96 }
97
98 int main(int argc, char **argv) {
99
100 ros::init(argc, argv, "tao_example_incrementer");
101
102 ROS_INFO("Starting...");
103
104 ros_decision_making_init(argc, argv);
105 ros::NodeHandle nodeHandle;
106 RosEventQueue eventQueue;
107
108 ros::AsyncSpinner spinner(1);
109 spinner.start();
110
111 LocalTasks::registrate("testTask", testTask);
112
113 eventQueue.async_spin();
114 CallContext call_ctx;
115 call_ctx.createParameters(new WorldModel());
116 TaoIncrementer(&call_ctx, &eventQueue);
117 eventQueue.close();
118 ROS_INFO("Finished.");
119 return 0;
120 }
Now let's break it down.
Our Plans are 'Incremental', 'Even', and 'Odd', Start and Stop conditions are Boolean as mentioned in the previous tutorial, and we have only one Allocation, for the Incremental plan.
These are the obvious inclusions, the TAO_HEADER for Odd and Even are mentioned here for C++ logic, so they will be known beforehand (just so everything will be recognized, so don't forget any of them!):
here we go:
1 #include <iostream>
2 #include <boost/bind.hpp>
3 #include <boost/thread.hpp>
4 #include <boost/foreach.hpp>
5 #include <ros/ros.h>
6
7 #include <decision_making/TAO.h>
8 #include <decision_making/TAOStdProtocols.h>
9 #include <decision_making/ROSTask.h>
10 #include <decision_making/DecisionMaking.h>
11
12 using namespace std;
13 using namespace decision_making;
14
15 #define foreach BOOST_FOREACH
16
17 TAO_HEADER(Even)
18 TAO_HEADER(Odd)
A definition of the context parameters for our world model, which will be used in our Incremental START conditions:
And now for the interesting part..
This is where TAO really comes in - We define the TAO plans for each component (according toTAO C++ reference as follows:
First for Incremental, our condition will be to run for 100 times (our context parameter will be incremented by our sub-plans).
notice the NEXT is the Incremental plan again, and our sub-plans are Odd and Even.
1 TAO(Incrementer)
2 {
3 TAO_PLANS{
4 Increment,
5 }
6 TAO_START_PLAN(Increment);
7 TAO_BGN{
8 TAO_PLAN( Increment ){
9 TAO_START_CONDITION( WM.i < 100 );
10 TAO_ALLOCATE(AllocFirstReady){
11 TAO_SUBPLAN(Even);
12 TAO_SUBPLAN(Odd);
13 }
14 TAO_STOP_CONDITION( false );
15 TAO_NEXT(NextFirstReady){
16 TAO_NEXT_PLAN(Increment);
17 }
18 }
19 }
20 TAO_END
21 }
And now for Even and Odd - after our Start condition we'll increment the context parameter, notice these are the last plans, so they have no TAO_ALLOCATE and no TAO_NEXT, we use the _EMPTY option instead.
Here we go:
1 TAO(Even)
2 {
3 TAO_PLANS{
4 Even,
5 }
6 TAO_START_PLAN(Even);
7 TAO_BGN
8 {
9 TAO_PLAN( Even ){
10 TAO_START_CONDITION( WM.i%2 == 0 );
11 WM.i++;
12 cout<<"Even : "<<WM.str()<<endl;
13 sleep(1);
14 TAO_ALLOCATE_EMPTY
15 TAO_STOP_CONDITION(true);
16 TAO_NEXT_EMPTY
17 }
18 }
19 TAO_END
20 }
21
22 TAO(Odd)
23 {
24 TAO_PLANS{
25 Odd,
26 }
27 TAO_START_PLAN(Odd);
28 TAO_BGN
29 {
30 TAO_PLAN( Odd ){
31 TAO_START_CONDITION( WM.i%2 != 0 );
32 WM.i++;
33 cout<<"Odd : "<<WM.str()<<endl;
34 sleep(1);
35 TAO_ALLOCATE_EMPTY
36 TAO_STOP_CONDITION(true);
37 TAO_NEXT_EMPTY
38 }
39 }
40 TAO_END
41 }
And for some basic running orders..
In the last part we'll define testTask for our result output, and run our main procedure: initialize ros and our program node, start the thread spinner, register the tasks, and call our TAO plan with the defined context parameters.
1 decision_making::TaskResult testTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
2 cout<<"[testTask from "<<context.str()<<"]"<<endl;
3 sleep(1);
4 return TaskResult::SUCCESS();
5 }
6
7 int main(int argc, char **argv) {
8
9 ros::init(argc, argv, "tao_example_incrementer");
10
11 ROS_INFO("Starting...");
12
13 ros_decision_making_init(argc, argv);
14 ros::NodeHandle nodeHandle;
15 RosEventQueue eventQueue;
16
17 ros::AsyncSpinner spinner(1);
18 spinner.start();
19
20 LocalTasks::registrate("testTask", testTask);
21
22 eventQueue.async_spin();
23 CallContext call_ctx;
24 call_ctx.createParameters(new WorldModel());
25 TaoIncrementer(&call_ctx, &eventQueue);
26 eventQueue.close();
27 ROS_INFO("Finished.");
28 return 0;
29 }
And that's it!
Using rqt_decision_graph
now let's run our rqt_decision_graph in parallel, to see the diagram our TAO program gives out:
